#include<ros/ros.h>
#include<geometry_msgs/PoseStamped.h>
#include<iostream>
int main(int argc, char** argv)
{
    ros::init(argc, argv, "publish_target_pose0");
    geometry_msgs::PoseStamped ps;
    ps.header.frame_id="frame_target";
    ros::NodeHandle nh;
    ros::Publisher target_publisher=nh.advertise<geometry_msgs::PoseStamped>("target/pose",5);

    float x,y,z,qx,qy,qz,qw;
    while(1)
    {
        std::cout<<"按照x,y,z,qw,qx,qy,qz 的顺序输入Pose!\n";
        std::cin>>x>>y>>z>>qx>>qy>>qz>>qw;
        ps.pose.position.x=x;
        ps.pose.position.y=y;
        ps.pose.position.z=z;
        ps.pose.orientation.w=qw;
        ps.pose.orientation.x=qx;
        ps.pose.orientation.y=qy;
        ps.pose.orientation.z=qz;
        ROS_INFO("going to the target pose:(%f,%f,%f) (%f,%f,%f,%f)",x,y,z,qw,qx,qy,qz);
        target_publisher.publish(ps);

    }
}
